package com.tgh.utils;

import java.util.List;

public class Math3D {

    private Math3D(){}
    /**
     * 旋转向量
     * @param result
     * @param vect
     * @param matrix
     */
    public static void rotateVector3(Vector3f result,Vector3f vect, Matrix33f matrix) {
        final float[] M=matrix.mMat;
        result.x=M[0]*vect.x+M[3]*vect.y+M[6]*vect.z;
        result.y=M[1]*vect.x+M[4]*vect.y+M[7]*vect.z;
        result.z=M[2]*vect.x+M[5]*vect.y+M[8]*vect.z;
    }
    /**
     * 旋转向量
     * @param result
     * @param vect
     * @param matrix
     */
    public static void rotateVector3(Vector3f result,Vector3f vect, Matrix44f matrix) {
        final float[] M=matrix.mMat;
        result.x=M[0]*vect.x+M[4]*vect.y+M[8]*vect.z;
        result.y=M[1]*vect.x+M[5]*vect.y+M[9]*vect.z;
        result.z=M[2]*vect.x+M[6]*vect.y+M[10]*vect.z;
    }
    /**
     * 平移向量
     * @param result
     * @param vect
     * @param matrix
     */
    public static void translateVector3(Vector3f result,Vector3f vect, Matrix44f matrix) {
        final float[] M=matrix.mMat;
        result.x=vect.x+M[12];
        result.y=vect.y+M[13];
        result.z=vect.z +M[14];
    }
    /**
     * 向量变幻
     * @param result
     * @param vect
     * @param matrix
     */
    public static void transformVector3(Vector3f result,Vector3f vect, Matrix44f matrix) {
        final float[] M=matrix.mMat;
        result.x=M[0]*vect.x+M[4]*vect.y+M[8]*vect.z+M[12];
        result.y=M[1]*vect.x+M[5]*vect.y+M[9]*vect.z+M[13];
        result.z=M[2]*vect.x+M[6]*vect.y+M[10]*vect.z +M[14];
    }
    /**
     * 向量变幻
     * @param result
     * @param vect
     * @param matrix
     */
    public static void transformVector4(Vector4f result,Vector4f vect, Matrix44f matrix) {
        final float[] M=matrix.mMat;
        result.x=M[0]*vect.x+M[4]*vect.y+M[8]*vect.z+M[12]*vect.w;
        result.y=M[1]*vect.x+M[5]*vect.y+M[9]*vect.z+M[13]*vect.w;
        result.z=M[2]*vect.x+M[6]*vect.y+M[10]*vect.z +M[14]*vect.w;
        result.w=M[3]*vect.x+M[7]*vect.y+M[11]*vect.z +M[15]*vect.w;
    }
    
    /**
     * 根据三个点(逆时针顺序)求平面方程
     * 右手坐标系用逆时针顺序的点，左手坐标系用顺时针顺序的点
     * planeEq(x,y,z)是平面的单位法向量n，w是d，平面方程是p·n=d
     */
    public static void getPlaneEquation(Vector4f planeEq,Vector3f p1,Vector3f p2,Vector3f p3) {
        Vector3f v1=new Vector3f();
        v1.x=p2.x-p1.x;
        v1.y=p2.y-p1.y;
        v1.z=p2.x-p1.z;
        Vector3f v2=new Vector3f();
        v2.x=p3.x-p1.x;
        v2.y=p3.y-p1.y;
        v2.z=p3.x-p1.z;
        Vector3f n = Vector3f.crossProduct(v1, v2);
        n.normalize();
        float d=Vector3f.dot(n, p1);
        planeEq.copy(n.x, n.y, n.z, d);
    }
    /**
     * 计算点集的最佳平面
     * @param vertices
     * @return
     */
    public static Vector4f computeBestFitPlane(Vector3f[] vertices) {
        Vector3f n=new Vector3f();
        int index=vertices.length-1;
        for(int i=0;i<vertices.length;i++){
            final Vector3f p=vertices[index];
            final Vector3f c=vertices[i];
            n.x+=(p.z+c.z)*(p.y-c.y);
            n.y+=(p.x+c.x)*(p.z-c.z);
            n.z+=(p.y+c.y)*(p.x-c.x);
            index=i;
        }
        n.normalize();
        Vector3f tmp=new Vector3f();
        for(int i=0;i<vertices.length;i++){
            Vector3f.add(tmp, tmp, vertices[i]);
        }
        float d=Vector3f.dot(tmp, n)/vertices.length;
        return new Vector4f(n.x, n.y, n.z, d);
    }
    /**
     * 为三角型造出法线，p1,p2,p3为逆时针顺序
     * @param p1
     * @param p2
     * @param p3
     * @return
     */
    public static Vector3f findNormal(Vector3f p1,Vector3f p2,Vector3f p3) {
        Vector3f v1= Vector3f.reduce(p1, p2);
        Vector3f v2=Vector3f.reduce(p2, p3);
        Vector3f normal = Vector3f.crossProduct(v1, v2);
        normal.normalize();
        return normal;
    }
    /**
     * 为三角型造出法线，p1,p2,p3为逆时针顺序
     * @param p1
     * @param p2
     * @param p3
     * @return
     */
    public static void findNormal(Vector3f result,Vector3f p1,Vector3f p2,Vector3f p3) {
        result.copy(findNormal(p1, p2, p3));
    }
    /**
     * 计算点到平面的有符号距离</BR>
     * 点在平面的正面之上，距离为正，反之，为负。
     * @param planeEq
     * @param point
     */
    public static float getDistanceToPlane(Vector3f point,Vector4f plane) {
        return point.x*plane.x+point.y*plane.y+point.z*plane.z-plane.w;
    }
    
    public static float[] vector2fToArray(Vector2f vect) {
        return new float[]{vect.x,vect.y};
    }
    
    public static float[] vector3fToArray(Vector3f vect) {
        return new float[]{vect.x,vect.y,vect.z};
    }
    
    public static float[] vector4fToArray(Vector4f vect) {
        return new float[]{vect.x,vect.y,vect.z,vect.w};
    }
    
    public static float[] vector2fsToArray(List<Vector2f> vects) {
        final int size=vects.size();
        float[] array=new float[size*2];
        for(int i=0;i<size;i++){
            final Vector2f vect=vects.get(i);
            array[i*2+0]=vect.x;
            array[i*2+1]=vect.y;
        }
        return array;
    }
    
    public static float[] vector2fsToArray(Vector2f[] vects) {
        final int size=vects.length;
        float[] array=new float[size*2];
        for(int i=0;i<size;i++){
            final Vector2f vect=vects[i];
            array[i*2+0]=vect.x;
            array[i*2+1]=vect.y;
        }
        return array;
    }
    
    public static float[] vector3fsToArray(List<Vector3f> vects) {
        final int size=vects.size();
        float[] array=new float[size*3];
        for(int i=0;i<size;i++){
            final Vector3f vect=vects.get(i);
            array[i*3+0]=vect.x;
            array[i*3+1]=vect.y;
            array[i*3+2]=vect.z;
        }
        return array;
    }
    
    public static float[] vector3fsToArray(Vector3f[] vects) {
        final int size=vects.length;
        float[] array=new float[size*3];
        for(int i=0;i<size;i++){
            final Vector3f vect=vects[i];
            array[i*3+0]=vect.x;
            array[i*3+1]=vect.y;
            array[i*3+2]=vect.z;
        }
        return array;
    }
    
    public static float[] vector4fsToArray(List<Vector4f> vects) {
        final int size=vects.size();
        float[] array=new float[size*4];
        for(int i=0;i<size;i++){
            final Vector4f vect=vects.get(i);
            array[i*4+0]=vect.x;
            array[i*4+1]=vect.y;
            array[i*4+2]=vect.z;
            array[i*4+3]=vect.w;
        }
        return array;
    }
    
    public static float[] vector4fsToArray(Vector4f[] vects) {
        final int size=vects.length;
        float[] array=new float[size*4];
        for(int i=0;i<size;i++){
            final Vector4f vect=vects[i];
            array[i*4+0]=vect.x;
            array[i*4+1]=vect.y;
            array[i*4+2]=vect.z;
            array[i*4+3]=vect.w;
        }
        return array;
    }
}
